#include <stdint-gcc.h>
#include <stddef.h>
#include "packet.h"
#include "ethmac.h"

/* Global variables */
mac_ip_t self_g = {
	{0x00, 0x0a, 0x35, 0x01, 0xf2, 0x5b},  /* MAC Address  */
	{192, 168, 1, 17}                      /* IPv4 address */
};

static packet_t _rx_packet_g;

packet_t *rx_packet_g = &_rx_packet_g;

void ethernet_header(char *buf, mac_t* target_mac, unsigned short type)
{
	/* ethernet header */
	/* DA */
	memcpy(&buf[0], target_mac, 6);
	// print_serial("%s:L%d ethernet header target mac %x:%x:%x:%x:%x:%x\n\r",
	//    __FILE__, __LINE__,
	//    target_mac->mac[0], target_mac->mac[1], target_mac->mac[2],
	//    target_mac->mac[3], target_mac->mac[4], target_mac->mac[5]
	//    );

	/* SA */
	memcpy(&buf[6], &self_g.mac[0], 6);
	buf[ 6] = self_g.mac[0];
	buf[ 7] = self_g.mac[1];
	buf[ 8] = self_g.mac[2];
	buf[ 9] = self_g.mac[3];
	buf[10] = self_g.mac[4];
	buf[11] = self_g.mac[5];

	/* type */
	buf[12] = type>>8;
	buf[13] = type&0xff;
}


void ip_header(char *buf, ip_t* target_ip, unsigned short ip_len, char ip_proto)
{
	unsigned short header_checksum;
	static unsigned short ip_id = 0;

	/* Version, Header length */
	buf[0] = 0x45;

	/* dscp */
	buf[1] = 0;

	/* ip len */
	buf[2] = ip_len>>8;
	buf[3] = ip_len&0xff;

	/* ID */
	buf[4] = (ip_id>>8)&0xff;
	buf[5] = ip_id&0xff;
	//ip_id++;

	/* Fragment */
	buf[6] = 0;
	buf[7] = 0;

	/* ttl */
	buf[8] = 64;

	/* Protocol */
	buf[9] = ip_proto;

	/* header checksum */
	buf[10] = 0;
	buf[11] = 0;

	/* Source IP */
	memcpy(&buf[12], &self_g.ip[0], 4);

	/* Destination IP */
	memcpy(&buf[16], target_ip, 4);

	/* header checksum */
	header_checksum = header_checksum16(buf, 20, 0);
	buf[10] = (header_checksum>>8)&0xff;
	buf[11] = header_checksum&0xff;
}



void arp_reply(mac_t* arp_sender_mac, ip_t* arp_sender_ip)
{
	char buf [44];
	ethernet_header(buf, arp_sender_mac, 0x0806);

	/* Hardware Type */
	buf[14] = 0x00;
	buf[15] = 0x01;
	/* Protocol Type */
	buf[16] = 0x08;
	buf[17] = 0x00;
	/* HLEN */
	buf[18] = 0x06;
	/* PLEN */
	buf[19] = 0x04;

	/* Operation = Reply */
	buf[20] = 0x00;
	buf[21] = 0x02;

	/* Sender MAC */
	buf[22] = self_g.mac[0];
	buf[23] = self_g.mac[1];
	buf[24] = self_g.mac[2];
	buf[25] = self_g.mac[3];
	buf[26] = self_g.mac[4];
	buf[27] = self_g.mac[5];

	/* Sender IP */
	buf[28] = self_g.ip[0];
	buf[29] = self_g.ip[1];
	buf[30] = self_g.ip[2];
	buf[31] = self_g.ip[3];

	/* Target MAC */
	buf[32] = arp_sender_mac->mac[0];
	buf[33] = arp_sender_mac->mac[1];
	buf[34] = arp_sender_mac->mac[2];
	buf[35] = arp_sender_mac->mac[3];
	buf[36] = arp_sender_mac->mac[4];
	buf[37] = arp_sender_mac->mac[5];

	/* Target IP */
	buf[38] = arp_sender_ip->ip[0];
	buf[39] = arp_sender_ip->ip[1];
	buf[40] = arp_sender_ip->ip[2];
	buf[41] = arp_sender_ip->ip[3];
	ethmac_tx_packet(buf, 42);
}


void ping_reply(packet_t* rx_packet, int ping_id, int ping_seq, char * rxbuf)
{

	int i;
	unsigned short header_checksum;
	// mac_ip_t target;
	char buf [96];  // not sure how long this needs  to be = (char*)ETHMAC_TX_BUFFER;

	ethernet_header(buf, (mac_t*) rx_packet->src_mac, 0x0800);  /*bytes 0 to 13*/
	ip_header(&buf[14], (ip_t*) rx_packet->src_ip, rx_packet->ip_len, 1); /* bytes 14 to 33, ip_proto = 1, ICMP*/

	/* ICMP */
	/* Type = reply */
	buf[34] = 0;

	/* Code = 0 */
	buf[35] = 0;

	/* checksum */
	buf[36] = 0;
	buf[37] = 0;

	/* ID */
	buf[38] = ping_id>>8;
	buf[39] = ping_id&0xff;

	/* SEQ */
	buf[40] = ping_seq>>8;
	buf[41] = ping_seq&0xff;

	for (i=8; i< rx_packet->ip_len - rx_packet->ip_header_len*4; i++) {
		buf[34+i] = rxbuf[i];
	}

	header_checksum = header_checksum16(&buf[34], (rx_packet->ip_len)-20, 0);
	buf[36] = (header_checksum>>8)&0xff;
	buf[37] = header_checksum&0xff;
	ethmac_tx_packet(buf, rx_packet->ip_len+14);
}


void parse_rx_packet(char * buf, packet_t* rx_packet)
{
	int i;

	for (i=0;i<6;i++)
		rx_packet->dst_mac[i] = buf[i];

	for (i=0;i<6;i++)
		rx_packet->src_mac[i] = buf[i+6];

	rx_packet->eth_type   = (buf[12]<<8) + buf[13];


	/* ARP */
	if (rx_packet->eth_type == 0x0806) {
		parse_arp_packet(&buf[14]);
	}

	/* Internet Protocol  */
	else if (rx_packet->eth_type == 0x0800){
		parse_ip_packet(&buf[14], rx_packet);
	}
}


void parse_arp_packet(char * buf)
{
	/* ARP is a broadcast message (mac broadcast address)
	   asking 'does this IP address belong to you?"
	 */
	int arp_op;
	//mac_ip_t arp_sender, arp_target;
	mac_t arp_sender_mac;
	ip_t arp_sender_ip;

	arp_op = buf[6]<<8 | buf[7];

	memcpy(&arp_sender_mac, &buf[8], 6);
	memcpy(&arp_sender_ip, &buf[14], 4);

	// arp_sender.mac[0] = buf[8];
	// arp_sender.mac[1] = buf[9];
	// arp_sender.mac[2] = buf[10];
	// arp_sender.mac[3] = buf[11];
	// arp_sender.mac[4] = buf[12];
	// arp_sender.mac[5] = buf[13];

	// arp_sender.ip [0] = buf[14];
	// arp_sender.ip [1] = buf[15];
	// arp_sender.ip [2] = buf[16];
	// arp_sender.ip [3] = buf[17];

	// arp_target.mac[0] = buf[18];
	// arp_target.mac[1] = buf[19];
	// arp_target.mac[2] = buf[20];
	// arp_target.mac[3] = buf[21];
	// arp_target.mac[4] = buf[22];
	// arp_target.mac[5] = buf[23];

	// arp_target.ip [0] = buf[24];
	// arp_target.ip [1] = buf[25];
	// arp_target.ip [2] = buf[26];
	// arp_target.ip [3] = buf[27];

	/* Send a reply ? */
	if (arp_op==1 &&
	    buf[24]==self_g.ip[0] &&
	    buf[25]==self_g.ip[1] &&
	    buf[26]==self_g.ip[2] &&
	    buf[27]==self_g.ip[3]) {

		// ARP reply
		arp_reply(&arp_sender_mac, &arp_sender_ip);
	}
}



void parse_ip_packet(char * buf, packet_t* rx_packet)
{
	unsigned int ip_version;

	ip_version = buf[0]>>4;
	if (ip_version != 4) {
		//print_serial("%s:L%d IP version %d not supported\n\r", __FILE__, __LINE__, ip_version);
		return;
	}

	/* Get destination IP address */
	rx_packet->dst_ip[0] = buf[16];
	rx_packet->dst_ip[1] = buf[17];
	rx_packet->dst_ip[2] = buf[18];
	rx_packet->dst_ip[3] = buf[19];

	/* If its not my address then ignore the packet */
	if (rx_packet->dst_ip[0] != self_g.ip[0] ||
	    rx_packet->dst_ip[1] != self_g.ip[1] ||
	    rx_packet->dst_ip[2] != self_g.ip[2] ||
	    rx_packet->dst_ip[3] != self_g.ip[3] ) {
		return;
	}

	rx_packet->ip_len         = buf[ 2]<<8|buf[ 3];
	rx_packet->ip_header_len  = buf[0] & 0xf;
	rx_packet->ip_proto       = buf[9];
	rx_packet->src_ip[0]      = buf[12];
	rx_packet->src_ip[1]      = buf[13];
	rx_packet->src_ip[2]      = buf[14];
	rx_packet->src_ip[3]      = buf[15];


	/* Ping packets */
	if (rx_packet->ip_proto == 1){
		parse_ping_packet(&buf[(rx_packet->ip_header_len)*4], rx_packet);
	}

#if 0
	/* TCP packets */
	else if (rx_packet->ip_proto == 6){
		parse_tcp_packet(&buf[(rx_packet->ip_header_len)*4], rx_packet);
	}

	/* UDP packets */
	else if (rx_packet->ip_proto == 17){
		parse_udp_packet(&buf[(rx_packet->ip_header_len)*4], rx_packet);
	}
#endif
}


void parse_ping_packet(char * buf, packet_t* rx_packet)
{
	int ping_id;
	int ping_seq;

	ping_id     = buf[4]<<8|buf[5];
	ping_seq    = buf[6]<<8|buf[7];

	ping_reply(rx_packet, ping_id, ping_seq, buf);
}


unsigned short header_checksum16(unsigned char *buf, unsigned short len, unsigned int sum)
{
	// build the sum of 16bit words
	while(len>1) {
		sum += 0xFFFF & (*buf<<8|*(buf+1));
		buf+=2;
		len-=2;
	}
	// if there is a byte left then add it (padded with zero)
	if (len) {
		sum += (0xFF & *buf)<<8;
	}
	// now calculate the sum over the bytes in the sum
	// until the result is only 16bit long
	while (sum>>16) {
		sum = (sum & 0xFFFF)+(sum >> 16);
	}

	// build 1's complement:
	return( (unsigned short ) sum ^ 0xFFFF);
}

void ping(uint8_t *dst_mac, uint32_t dst_ip)
{
	int i;
	unsigned short header_checksum;
	// mac_ip_t target;
	char buf[50];

	ethernet_header(buf, (mac_t*)dst_mac, 0x0800);  /*bytes 0 to 13*/
	ip_header(&buf[14], (ip_t*)&dst_ip, 36, 1); /* bytes 14 to 33, ip_proto = 1, ICMP*/

	/* ICMP */
	/* Type = Echo request */
	buf[34] = 8;

	/* Code = 0 */
	buf[35] = 0;

	/* checksum */
	buf[36] = 0;
	buf[37] = 0;

	/* ID */
	buf[38] = 0x55;
	buf[39] = 0xaa;

	/* SEQ */
	buf[40] = 0x55;
	buf[41] = 0xaa;

	/* Timestamp */
	for (i=42; i<50; i++) {
		buf[i] = 0;
	}

	header_checksum = header_checksum16(&buf[34], 16, 0);
	buf[36] = header_checksum >> 8;
	buf[37] = header_checksum;
	ethmac_tx_packet(buf, 50);
}
